// --------------------------------------------------------------------------------------------------------------------
// <copyright file="InverseKinematicsNumerical.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using ShoNS.Array;

    /// <summary>
    /// A class to solve the inverse kinematics of a kinematic chain using NEWTON-RAPHSON method. (http://en.wikipedia.org/wiki/Newton's_method)
    /// The solution is a list of joint configurations.
    /// </summary>
    public class InverseKinematicsNumerical
    {
        /// <summary>
        /// Default maximum number of iteration for numerical IK
        /// </summary>
        private const int DefaultMaxNumberOfIKIteration = 50;

        /// <summary>
        /// Default number of steps to divide the range of a joint
        /// </summary>
        private const int DefaultSeedJointConfigurationSamplingResolution = 100;

        /// <summary>
        /// Default translational tolerance on Cartesian move, in meter
        /// </summary>
        private const double DefaultCartesianMoveTranslationalTolerance = 0.001;

        /// <summary>
        /// Default rotational tolerance on Cartesian move, in radian
        /// </summary>
        private const double DefaultCartesianMoveRotationalTolerance = 0.001;

        /// <summary>
        /// Default maximum joint angle change in one single step/iteration
        /// </summary>
        private const double DefaultMaximumSingleJointChangeInRadian = 5 * MathConstants.Degrees2Radians;

        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext ikContext = new TraceContext(0, "IK", TraceContexts.Manip);

        /// <summary>
        /// A delegate which generates other inverse kinematics solutions in which the end
        /// effector poses are equivalent.
        /// The returned list need not be an exhaustive list or even a regular sampling of the
        /// entire null space of the arm.  This could be done based on geometric similarities
        /// or via other methods (sampling the null space).
        /// This delegate is not supposed to do any heavy computation as a numerical IK
        /// algorithm does.
        /// Please refer to the function KukaLBR4EquivalentJointConfigurationGenerator
        /// inside KukaLBR4KinematicConfigurations for a practical implementation.
        /// </summary>
        /// <param name="jointValues">Current inverse kinematics solution</param>
        /// <returns>A list of other inverse kinematics solutions that are equivalent in the sense
        /// that the end effector pose of the arm for all these solutions remains the same</returns>
        public delegate List<DoubleArray> InverseKinematicsExtender(DoubleArray jointValues);

        /// <summary>
        /// Calculate the joint update using simple jacobian inverse
        /// </summary>
        /// <param name="jacobian">Current jacobian matrix</param>
        /// <param name="poseDiff">Pose difference</param>
        /// <returns>A joint update matrix</returns>
        public static DoubleArray GetPlainJacobianInverseJointChange(DoubleArray jacobian, DoubleArray poseDiff)
        {
            SVD svd = new SVD(jacobian);
            DoubleArray inv = svd.PseudoInverse();

            // compute changes to the joint values at this step
            DoubleArray jointChanges = inv * poseDiff;

            return jointChanges;
        }

        /// <summary>
        /// Using numerical method to solve the IK problem given one seed
        /// </summary>
        /// <param name="kc">The kinematic chain</param>
        /// <param name="endEffectorPose">The target end effector pose</param>
        /// <param name="seed">Seed joint configuration, column vector</param>
        /// <param name="maxNumIterationsExceeded">A boolean variable that indicates whether the max iteration limit is hit</param>
        /// <param name="jointsWithinRange">A boolean variable that indicates whether each joint is within the valid range</param>
        /// <param name="translationalTolerance">Translational tolerance</param>
        /// <param name="rotationalTolerance">Rotational tolerance</param>
        /// <param name="maxNumIterations">Maximum number of iterations</param>
        /// <param name="maxJointChangeInRadian">Maximum joint change in one iteration</param>
        /// <returns>A list of IK solutions, which is a list of joint configurations</returns>
        public static DoubleArray SolveIKNumerical(
            KinematicChain kc,
            Matrix4 endEffectorPose,
            DoubleArray seed,
            out bool maxNumIterationsExceeded,
            out bool jointsWithinRange,
            double translationalTolerance = InverseKinematicsNumerical.DefaultCartesianMoveTranslationalTolerance,
            double rotationalTolerance = InverseKinematicsNumerical.DefaultCartesianMoveRotationalTolerance,
            int maxNumIterations = InverseKinematicsNumerical.DefaultMaxNumberOfIKIteration,
            double maxJointChangeInRadian = InverseKinematicsNumerical.DefaultMaximumSingleJointChangeInRadian)
        {
            TraceOut.Info(ikContext, "IK request EE Transform: {0}", endEffectorPose);
            TraceOut.Info(ikContext, "IK Seed: {0}", seed.Transpose());

            // Compute the target pose for the last joint which is defined inside the kinematic chain.
            // Because the Jacobian matrix considers the last joint as the reference point, we need to
            // calculate the pose of the last joint for the IK problem.
            Matrix4 targetLastJointPose = endEffectorPose * HomogenousTransform.Inverse(kc.EndEffectorOffsetInLastJoint);

            DoubleArray jointValues = seed;
            maxNumIterationsExceeded = false;
            DoubleArray jointChanges;
            PoseDiff poseDiff;
            int stepSoFar = 0;
            while (true)
            {
                // compute the current pose of the last joint
                Matrix4 currentLastJointPose = kc.GetJointTransforms(jointValues)[kc.JointList.Length - 1];

                // compute the pose difference
                poseDiff = new PoseDiff(currentLastJointPose, targetLastJointPose);

                // compute the Jacobian matrix
                DoubleArray jacobian = kc.GetLastJointJacobianMatrix(jointValues);

                jointChanges = GetPlainJacobianInverseJointChange(jacobian, poseDiff);

                double maxAbsoluteJointChange = MatrixExtensions.MaxAbsoluteValue(jointChanges);

                // if the changes are too small, we decide that we are finished
                // if we hit the iteration limit, we have to exit
                double threshold = MathConstants.ErrorEpsilon;
                if (maxAbsoluteJointChange < threshold || stepSoFar > maxNumIterations)
                {
                    maxNumIterationsExceeded = (MatrixExtensions.MaxAbsoluteValue(jointChanges) >= threshold);

                    // one last step to check on the difference of pose
                    // even though the safe guard is exceeded, we can check
                    // whether the current pose is close enough to the requested
                    // pose by examining the poseDiff
                    if (maxNumIterationsExceeded)
                    {
                        // if the difference is within the tolerance, we decide that we are finished
                        bool poseDiffWithinTolerance = InverseKinematicsNumerical.CheckPoseDifferenceWithinTolerance(translationalTolerance, rotationalTolerance, poseDiff);

                        // if the difference is within tolerance, we still consider this solution to be valid
                        if (poseDiffWithinTolerance)
                        {
                            maxNumIterationsExceeded = false;
                        }
                    }

                    break;
                }

                // if one step is too big, we scale it down for joint update
                if (maxAbsoluteJointChange > maxJointChangeInRadian)
                {
                    double scale = maxJointChangeInRadian / maxAbsoluteJointChange;
                    jointChanges = jointChanges * scale;
                }

                // apply the changes to the joint values
                jointValues = jointValues + jointChanges;

                // constrain joint values to be within joint limits
                ConstrainJointValuesToJointLimits(jointValues, kc.JointList);

                // update step counter
                ++stepSoFar;
            }

            // put all the joints to [-pi, pi]
            jointsWithinRange = NormalizeSolutionAndCheckWithJointLimits(jointValues, kc);

            TraceOut.Info(ikContext, "joint change at last step {0}", jointChanges.Transpose());
            TraceOut.Info(ikContext, "pose diff: {0}", poseDiff.Transpose());
            TraceOut.Info(ikContext, "steps at break: {0}", stepSoFar);
            TraceOut.Info(ikContext, "solution {0}", jointValues.Transpose());

            return jointValues;
        }

        /// <summary>
        /// Using numerical method to solve the IK problem given a number of seeds for initial joint configurations.
        /// Random joint seeds will be generated within this method.
        /// Up to "numSeeds" solutions will be output.
        /// </summary>
        /// <param name="kc">The kinematic chain</param>
        /// <param name="endEffectorPose">The pose of the end effector</param>
        /// <param name="numSeeds">The number of seeds for the IK computation</param>
        /// <param name="translationalTolerance">Translational tolerance</param>
        /// <param name="rotationalTolerance">Rotational tolerance</param>
        /// <param name="extender">Inverse kinematics solution extender</param>
        /// <param name="maxNumIterations">Maximum number of iterations</param>
        /// <param name="jointSamplingResolution">The sampling resolution for the seeds</param>
        /// <param name="maxJointChangeInRadian">Maximum joint change in one iteration</param>
        /// <returns>A list of valid solutions for joint configurations</returns>
        public static List<DoubleArray> SolveIKNumerical(
            KinematicChain kc,
            Matrix4 endEffectorPose,
            int numSeeds,
            double translationalTolerance = InverseKinematicsNumerical.DefaultCartesianMoveTranslationalTolerance,
            double rotationalTolerance = InverseKinematicsNumerical.DefaultCartesianMoveRotationalTolerance,
            InverseKinematicsExtender extender = null,
            int maxNumIterations = InverseKinematicsNumerical.DefaultMaxNumberOfIKIteration,
            int jointSamplingResolution = InverseKinematicsNumerical.DefaultSeedJointConfigurationSamplingResolution,
            double maxJointChangeInRadian = InverseKinematicsNumerical.DefaultMaximumSingleJointChangeInRadian)
        {
            List<DoubleArray> solutions = new List<DoubleArray>();
            for (int i = 0; i < numSeeds; ++i)
            {
                // randomly generate an initial joint configuration
                DoubleArray seed = GenerateRandomJointConfiguration(kc, jointSamplingResolution);

                bool maxNumIterationsExceeded;
                bool jointsWithinRange;

                // solve the IK problem based on the generated seed joint configuration
                DoubleArray solution = SolveIKNumerical(kc, endEffectorPose, seed, out maxNumIterationsExceeded, out jointsWithinRange, translationalTolerance, rotationalTolerance, maxNumIterations, maxJointChangeInRadian);

                // if the safe guard is exceeded or joints are not within range
                // we skip this solution
                if (maxNumIterationsExceeded || !jointsWithinRange)
                {
                    continue;
                }

                // collect solutions
                if (null != extender)
                {
                    solutions.AddRange(extender(solution));
                }
                else
                {
                    solutions.Add(solution);
                }
            }

            return solutions;
        }

        /// <summary>
        /// Check a given pose difference is within the given tolerance
        /// </summary>
        /// <param name="translationalTolerance">Translational tolerance</param>
        /// <param name="rotationalTolerance">Rotational tolerance</param>
        /// <param name="poseDiff">Pose difference</param>
        /// <returns>Whether the pose difference is within tolerance</returns>
        public static bool CheckPoseDifferenceWithinTolerance(double translationalTolerance, double rotationalTolerance, PoseDiff poseDiff)
        {
            DoubleArray translationalDiff = poseDiff.TranslationalDifference();
            DoubleArray rotationalDiff = poseDiff.RotationalDifference();
            double translationalError = translationalDiff.Norm();
            double rotationalError = rotationalDiff.Norm();
            bool poseDiffWithinTolerance = (translationalError < translationalTolerance) && (rotationalError < rotationalTolerance);

            TraceOut.Info(ikContext, "error {0}, {1}", translationalError, rotationalError);
            TraceOut.Info(ikContext, "tolerance {0}, {1}", translationalTolerance, rotationalTolerance);

            return poseDiffWithinTolerance;
        }

        /// <summary>
        /// Generates a list of random joint configurations for a kinematic chain given its joint list.
        /// These joint configurations can be used as seeds to the numerical IK solver.
        /// </summary>
        /// <param name="kc">The kinematic chain</param>
        /// <param name="samplingResolution">The resolution we want to have for sampling the joints</param>
        /// <param name="generationSeed">A seed for random number generation. It will generate the same sequence of
        /// random numbers when given the same generation seed.</param>
        /// <returns>A list of joint configurations</returns>
        public static DoubleArray GenerateRandomJointConfiguration(
            KinematicChain kc,
            int samplingResolution = InverseKinematicsNumerical.DefaultSeedJointConfigurationSamplingResolution,
            int generationSeed = -1)
        {
            double[] steps = new double[kc.JointList.Length];
            
            System.Random r;
            if (generationSeed > 0)
            {
                r = new Random(generationSeed);
            }
            else
            {
                r = new Random();
            }

            for (int i = 0; i < steps.Length; ++i)
            {
                // randomly generates a joint step
                steps[i] = r.Next(0, samplingResolution);
            }

            // setup the joint values based on the step
            DoubleArray jointValues = new DoubleArray(kc.JointList.Length, 1);
            for (int i = 0; i < jointValues.Length; ++i)
            {
                jointValues[i, 0] = kc.JointList[i].MinValue + (kc.JointList[i].MaxValue - kc.JointList[i].MinValue) * steps[i] / samplingResolution;

                jointValues[i, 0] = jointValues[i, 0];
            }

            return jointValues;
        }

        /// <summary>
        /// Normalize all the joints to make sure they are within [-pi,pi].
        /// Check the validity of a solution by comparing each joint value against
        /// the corresponding joint angle range.
        /// </summary>
        /// <param name="solution">Reorganized solution in a column vector</param>
        /// <param name="kc">A kinematic chain, when a kinematic chain is not given, this method only constrains
        /// the joints to be within [-pi,pi].</param>
        /// <returns>Whether this solution is valid</returns>
        public static bool NormalizeSolutionAndCheckWithJointLimits(DoubleArray solution, KinematicChain kc)
        {
            bool valid = true;
            for (int i = 0; i < solution.size0; ++i)
            {
                // bring it to [0, 2*pi]
                solution[i, 0] = solution[i, 0] % MathConstants.TwoPI;

                // bring it to [-pi, pi]
                if (solution[i, 0] < -Math.PI)
                {
                    solution[i, 0] += MathConstants.TwoPI;
                }

                if (solution[i, 0] > Math.PI)
                {
                    solution[i, 0] -= MathConstants.TwoPI;
                }

                // check if this joint angle is within the joint's limit if kc is set
                if (null != kc)
                {
                    if (solution[i, 0] > kc.JointList[i].MaxValue || solution[i, 0] < kc.JointList[i].MinValue)
                    {
                        valid = false;
                    }
                }
            }

            return valid;
        }

        /// <summary>
        /// Constrain joint values to joint limits
        /// </summary>
        /// <param name="jointValues">joint values to constrain, may be modified after this function is called</param>
        /// <param name="jointList">A list of the joints of the kinematic chain</param>
        private static void ConstrainJointValuesToJointLimits(DoubleArray jointValues, Joint[] jointList)
        {
            for (int i = 0; i < jointValues.size0; ++i)
            {
                jointValues[i, 0] = jointValues[i, 0] % MathConstants.TwoPI;
                if (jointValues[i, 0] > jointList[i].MaxValue)
                {
                    jointValues[i, 0] = jointList[i].MaxValue;
                }
                else if (jointValues[i, 0] < jointList[i].MinValue)
                {
                    jointValues[i, 0] = jointList[i].MinValue;
                }
            }
        }
    }
}
